#pragma once
// write by lbf_heu 2023/4/15 

// 进行地形的仿真数据：地形是一个xy的sin cos 函数模拟


#include "bits/stdc++.h"

#include "eigen3/Eigen/Core"


#include "viewer.hpp"
#include "head.hpp"
#include "path.hpp"
#include "robot.hpp"
#include "opt.hpp"


class Simer
{
public:
    using EV3d = Eigen::Vector3d;
    robots * robot;
    
    viewer * show;
    path * pather;
    opt * opter;

    double t = 0.; 
    // 根据实际位置模拟激光点的数据 并进行优化
    void getRobotPosAndTrans(std::vector<Eigen::Vector3d> walls){
        Eigen::Vector3d pt(0.5, 0.5, 0.);

        // 拿到真实的pos和trans
        Eigen::Vector3d trans;
        Eigen::Matrix3d pos;
        pather->getPosAndTrans(t, pos, trans);  
        
        std::vector<Eigen::Vector9d> lasers = dynamic_cast<threeWheelRobots*>(robot)->getLaserData(pos, trans);// 拿到激光的数据点
        std::vector<Eigen::Vector6d> lasersDir = dynamic_cast<threeWheelRobots*>(robot)->getLaserDirection(pos, trans);

        show->setLasers(lasers);
        show->setLasersLoaction(lasersDir);
        
        std::vector<double> disVec; 
        for(int i = 0; i < lasers.size(); i++){
            disVec.push_back(lasers[i](3, 0));// 拿到在laser坐标系下的x值
        }
        opter->optPosAndTrans(pt, disVec, walls);
        show->setPathPoint(pt);
        show->setPosAndTrans(pos, trans);// 显示机器人的真实运动情况
        usleep(100000);   // sleep 0.1s
        t += 0.05;
        if(t > 100.) t = 0.;
        
    }
    // 对数据进行求解与标定，进行一个全局的ba过程
    void getRobotPosAndTransAndExt(std::vector<Eigen::Vector3d> walls){
        // opter->chackErrorAndJacobian();
        // while(1);
        // step 1 收集500组数据
        std::vector<std::vector<Eigen::Vector9d>> Vlasers;
        std::vector<std::vector<Eigen::Vector6d>> VlasersDir;
        std::vector<std::pair<Eigen::Vector3d, Eigen::Matrix3d>> VposAndTrans;
        const int maxDataNum = 1000;// 收集的最大数据量
        int inum = 0;
        for(double t = 0.; inum < maxDataNum; t += 0.1, inum += 1){
            // 拿到真实的pos和trans
            Eigen::Vector3d trans;
            Eigen::Matrix3d pos;
            pather->getPosAndTrans(t, pos, trans);  
            VposAndTrans.emplace_back(trans, pos);
            // 采样激光的数据信息
            std::vector<Eigen::Vector9d> lasers = dynamic_cast<threeWheelRobots*>(robot)->getLaserData(pos, trans);// 拿到激光的数据点
            std::vector<Eigen::Vector6d> lasersDir = dynamic_cast<threeWheelRobots*>(robot)->getLaserDirection(pos, trans);
            // 把数据信息保存下来
            Vlasers.push_back(lasers);
            VlasersDir.push_back(lasersDir);
        }
        std::vector<Eigen::Vector3d> ptVec(Vlasers.size(), Eigen::Vector3d(0.5, 0.5, 0.));// 定义数据优化出来的结果
        std::vector<Eigen::Vector3d> laserExt; // 待优化的外参参数 
        dynamic_cast<threeWheelRobots*>(robot)->get2dLaserPosAndTrans(laserExt);
        
        // 进行全局优化的过程
        
        opter->optPosAndTransAndEXt(walls, Vlasers, ptVec, laserExt);
        std::cout << std::endl;
        for(size_t ext = 0; ext < laserExt.size(); ++ext){
            std::cout << ext << " ext = " << laserExt[ext].transpose() << std::endl;
        }

        // 进行全局可视化的过程
        for(size_t inum = 0; ; inum +=1 ){// 一直在这一个区域回滚
            // std::cout << "inum = " << inum << std::endl;
            inum = inum % maxDataNum;
            // 显示激光的测量信息
            auto & lasers = Vlasers[inum];
            show->setLasers(lasers);
            // 显示激光器的方向
            auto & lasersDir = VlasersDir[inum];
            show->setLasersLoaction(lasersDir);
            auto & pt = ptVec[inum];
            show->setPathPoint(pt);// 显示优化出来的路径结果
            Eigen::Vector3d trans = VposAndTrans[inum].first;
            Eigen::Matrix3d pos = VposAndTrans[inum].second;
            show->setPosAndTrans(pos, trans);// 显示机器人的真实运动情况
            usleep(100000);   // sleep 0.1s
        }
        
    }
    // 定义机器人的位姿 获得机器人的三个轮子的位置
    Simer(robots * _robot, viewer * _show, path * _pather):
    robot(_robot), show(_show), pather(_pather){
        opter = new opt(_robot);
    }
    ~Simer(){

    }
};


